#include "pathplanwidget.h"
#include <QFileDialog>
#include <QProcess>
#include "util.h"
#include "RTLSDisplayApplication.h"
#include "views/toast.h"
#include <QLabel>
#include "leiliconfig.h"
#include <QTimer>
#include <visualization_msgs/Marker.h>
#include "util/rosbag.h"
#include "util/topicfiled.h"



namespace  {
    QString CMD_runMapServer = "rosrun map_server map_server";
    QString CMD_runPathPlanNode = "roslaunch MSTC_Star_update   main_leili.launch  ";
};

#define MAX_COLOR 16581375
PathPlanWidget::PathPlanWidget(QWidget *parent) : QWidget(parent)
{
    setupUI();

    m_node = new RosNode();
    m_node->subMap();
    m_node->subPose();
    m_node->subPath();

    m_process = new cmdProcess(this);

    m_processPath = new cmdProcess(this);

    m_mapFileManger = new ros_map_edit::MapFileManager();

    //m_mapTrans = new MapTransformer(this);

    m_pubMarkerTimer = new QTimer(this);
    m_pubMarkerTimer->setInterval(100);

    createConnect();
}

PathPlanWidget::~PathPlanWidget()
{
    delete m_node;
    m_processPath->clearCmd();

    if(m_mapTrans != nullptr)
    {
         delete m_mapTrans;
        m_mapTrans = nullptr;
    }

    delete m_mapFileManger;
}


void PathPlanWidget::setupUI()
{
    QVBoxLayout *vl =new QVBoxLayout(this);
    vl->setContentsMargins(0,0,0,0);

    m_rviz = new QRviz(this);
    m_rviz->Display_Grid(true, "<Fixed Frame>",QColor(160,160,164));
    m_rviz->Display_Axes(true,"<Fixed Frame>","2","0.3");
    m_rviz->Display_Map(true, QString::fromStdString(TopicFiled::map), 0.7, "map");

    m_rviz->Show_TopDownView();


    QHBoxLayout *vH1 = new QHBoxLayout(this);
    vH1->addStretch();

    QLabel *lable =  new QLabel(this);
    lable->setText("初始位姿:");
    vH1->addWidget(lable);

    for(int i =1 ; i < LeiliCfg->getRobotNum() + 1; i++)
    {
        QColor color(Qt::GlobalColor(Qt::red + i -1));

        QPushButton *p = new QPushButton(this);
        p->setText(QString("机器人%1").arg(i));

        QPalette palette = p->palette();
        palette.setColor(QPalette::ButtonText, color);
        p->setPalette(palette);
        p->setAutoFillBackground(true);



        QObject::connect(p, &QPushButton::clicked, [&,i](){
            m_rviz->Set_Pos(QString("/robot%1/checkPose").arg(i));//执行2D pose estimate操作
        });

        m_rviz->Display_PoseWithCov(true, QString("robot%1").arg(i), QString("/robot%1/checkPose").arg(i), color);//订阅2D pose estimate操作
        m_rviz->Display_Path(QString("/robot%1/path").arg(i), color, 0.06);
        m_rviz->Display_Marker("visualization_marker",color);


        vH1->addWidget(p);

    }

    vH1->addSpacing(48);
    m_btnSend = new QPushButton(this);
    m_btnSend->setText("规划");

    m_btnBack2Zero = new QPushButton(this);
    m_btnBack2Zero->setText("回到原点");


    vH1->addWidget(m_btnSend);
    vH1->addWidget(m_btnBack2Zero);
    vH1->addStretch();


    QHBoxLayout *hB = new QHBoxLayout(this);
    m_btnOpenFile = new QPushButton(this);
    m_btnOpenFile->setText("打开");

    m_btnSave = new QPushButton(this);
    m_btnSave->setText("保存");

    m_btnOpenRogBag = new QPushButton(this);
    m_btnOpenRogBag->setText("查看");

    hB->addStretch();
    hB->addWidget(m_btnOpenFile);
    hB->addWidget(m_btnSave);
    hB->addWidget(m_btnOpenRogBag);
    hB->addStretch();


    vl->addLayout(vH1);
    vl->addWidget(m_rviz);
    vl->addLayout(hB);
}

void PathPlanWidget::createConnect()
{
    QObject::connect(m_btnSend, &QPushButton::clicked, this, &PathPlanWidget::onBtnSend);
    QObject::connect(m_btnOpenFile, &QPushButton::clicked, this, &PathPlanWidget::onBtnOpen);
    QObject::connect(m_btnBack2Zero, &QPushButton::clicked, this, &PathPlanWidget::onBack2Zero);
    QObject::connect(m_node, &RosNode::sigPoseSelectState, this, &PathPlanWidget::onPoseSelectState);

    QObject::connect(m_process, &cmdProcess::executeSucc, [=](){
        emit App->statusBarMessage(QtInfoMsg, QString("打开文件%1成功").arg(m_curfileName));

    });
    QObject::connect(m_process, &cmdProcess::executeFaild, [=](){
        emit App->statusBarMessage(QtWarningMsg, QString("打开文件%1失败").arg(m_curfileName));
    });

    QObject::connect(m_processPath, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), [=](int exitCode, QProcess::ExitStatus exitStatus){
        if(m_processPath->state() == QProcess::NotRunning)
        {
             //m_processPath->run(QStringList(CMD_runPathPlanNode));
        }
    });

    QObject::connect(m_processPath, &QProcess::stateChanged, [&](QProcess::ProcessState state){
        if(state == QProcess::Running)
        {
            //m_node->pubMap();
            m_node->pubPose();
        }
    });


     connect(m_pubMarkerTimer, &QTimer::timeout, this, &PathPlanWidget::onUpdateMarker);


     QObject::connect(m_btnSave, &QPushButton::clicked, this, &PathPlanWidget::onSave);
     QObject::connect(m_btnOpenRogBag, &QPushButton::clicked, this, &PathPlanWidget::onOpenPathBag);


}



void PathPlanWidget::onBtnSend()
{
    if(m_mapTrans != nullptr)
    {
         delete m_mapTrans;
        m_mapTrans = nullptr;
        m_pubMarkerTimer->stop();
    }

    float res = m_node->getMap().info.resolution;
    if(res <=0)
    {
        Toast::toast("warningImg", "栅格地图数据错误!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    vector<geometry_msgs::PoseWithCovarianceStamped> vecInitPose;
    m_node->getPos(vecInitPose);

    if(vecInitPose.size() < LeiliCfg->getRobotNum())
    {
        Toast::toast("warningImg", "机器人初始坐标没有全部设置正确!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    m_mapTrans = new MapTransformer();
    m_mapTrans->setRobotNum(LeiliCfg->getRobotNum());

    double scale  = (LeiliCfg->getGrindingPanDiameter() - LeiliCfg->getOverlapWidth()) /res;
    //m_mapTrans->setMapScale(scale);
    m_mapTrans->setMapScale(1/res);
    QString msg = QString("diame:%1, width:%2,sacle:%3").arg(LeiliCfg->getGrindingPanDiameter()).arg(LeiliCfg->getOverlapWidth()).arg(res);
    qInfo() << msg;

    m_mapTrans->setMap(m_node->getMap());

    bool succ = m_mapTrans->createPaths(vecInitPose);
    if(!succ)
    {
        Toast::toast("warningImg", "路径规划失败!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    auto path = m_mapTrans->getPath();
    m_pathResult = path;
    m_node->pubPath(path);
    //m_node->pubMarker(path);

    Toast::toast("succImg", "路径规划成功!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
    m_prePathIndex.clear();
    m_prePathIndex.resize(LeiliCfg->getRobotNum(),0);
    m_pubMarkerTimer->start();
}

void PathPlanWidget::onBtnOpen()
{

    m_curfileName = QFileDialog::getOpenFileName(this, "打开栅格地图", LeiliCfg->getSavePath(),
                                                     "yaml文件 (*.yaml)",nullptr,QFileDialog::DontUseNativeDialog);
     if (m_curfileName == "") return;


//    QStringList cmd;
//    cmd <<CMD_runMapServer + " " + m_curfileName;

//    m_process->run(cmd);
    nav_msgs::OccupancyGrid map;
    bool succ = m_mapFileManger->loadMap(m_curfileName.toStdString(),map);
    if(!succ)
    {
        Toast::toast("warningImg", QString("打开文件:%1失败!").arg(m_curfileName), QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    m_node->pubMap(map);
    Toast::toast("succImg", QString("打开文件:%1succ!").arg(m_curfileName), QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
}

void PathPlanWidget::onPoseSelectState(RosNode::PoseSelect errCode, int robotIndex)
{

    QString msg =QString("机器人%1").arg(robotIndex);
    QtMsgType type = QtInfoMsg;
    switch (errCode)
    {
    case RosNode::PoseSelect_Succ:
        msg += "选取坐标成功!";
        Toast::toast("succImg", msg, QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));

        break;
    case RosNode::PoseSelect_NotInMap:
        msg += "选取坐标不在地图内，请重新选择!";
        type = QtWarningMsg;
        Toast::toast("warningImg", msg, QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        break;
    case RosNode::PoseSelect_InObstacle:
        type = QtWarningMsg;
        msg += "选取坐标在障碍物上，请重新选择!";
        Toast::toast("warningImg", msg, QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        break;
    default:
        return;
     }


    emit App->statusBarMessage(type, msg);
}

void PathPlanWidget::onBack2Zero()
{
    m_rviz->Zero();
}

void PathPlanWidget::onUpdateMarker()
{
    int allSendCount = 0;
    for(int i = 0; i < m_prePathIndex.size(); i++)
   {
        int preIndex = m_prePathIndex[i];
        nav_msgs::Path& path = m_pathResult[i];

        if(preIndex >= (path.poses.size() -1))
         {

            allSendCount++;
            if(allSendCount == m_prePathIndex.size())
            {
                m_pubMarkerTimer->stop();
            }

            continue;
         }

          nav_msgs::Path pubPath;
          pubPath.header = path.header;
//           pubPath.poses.push_back(path.poses[preIndex]);
        for(int j =0; j < preIndex +1; j++)
        {
            pubPath.poses.push_back(path.poses[j]);
        }

        QColor color(Qt::GlobalColor(Qt::red + i));
        m_node->pubMarker(pubPath,i,visualization_msgs::Marker::LINE_STRIP,color);
        m_node->pubMarker(pubPath,i*10,visualization_msgs::Marker::POINTS,color);

//        nav_msgs::Path lastPoint;
//        lastPoint.header = path.header;

//        lastPoint.poses.push_back(path.poses[preIndex+1]);
//        m_node->pubMarker(lastPoint,i,visualization_msgs::Marker::POINTS);

        m_prePathIndex[i] = ++preIndex;
    }
}

void PathPlanWidget::onSave()
{
    if(m_pathResult.empty())
    {
        Toast::toast("warningImg", "没有要保存的路径!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    if(!m_curfileName.size())
    {
        Toast::toast("warningImg", "无法获取原始地图路径，请重新打开地图!", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    auto fileName  = QFileInfo(m_curfileName).baseName();
    auto saveName = LeiliCfg->getSavePath().toStdString() +"/" + fileName.toStdString() + ".bag";
    RosBagSaver::wirtePath(saveName,m_pathResult);
}

void PathPlanWidget::onOpenPathBag()
{
    QString bagFileName = QFileDialog::getOpenFileName(this, "打开栅格地图", LeiliCfg->getSavePath(),
                                                     "yaml文件 (*.bag)",nullptr,QFileDialog::DontUseNativeDialog);
     if (bagFileName == "") return;

     std::vector<nav_msgs::Path> path;
     RosBagSaver::readPath(bagFileName.toStdString(),path);
     m_node->pubPath(path);
}
